/*
 * caosensornode.cpp
 *
 *
 *  Created on: Oct 8, 2011
 *  Author: Mario H. Voorsluys
 *	email: forcaeluz@gmail.com
 *
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, visit:
 * http://www.gnu.org/copyleft/gpl.html
 */

#include <iostream>
#include <cstdlib>

#include "caosensornode.h"

CaoSensorNode::CaoSensorNode():node_handle("") {
	usb_device = (char*)"/dev/ttyUSBCao";
	init();
}

CaoSensorNode::CaoSensorNode(char* file_descriptor):node_handle(""){
	usb_device = file_descriptor;
	init();
}

CaoSensorNode::~CaoSensorNode() {
	node_handle.shutdown();
}

void CaoSensorNode::init(){

	/*
	 * Open the connection with the serial port. The file descriptot in my laptop is
	 * /dev/ttyUSB0
	 * In other computers it will probably something similar. It could be /dev/ttyS0 for example.
	 */
	bool check = false;
	while(!check){
		if(communicator.openConnection(usb_device)){
			check = true;
			std::cout << "Connection open" << std::endl;
		}else{
			std::cout << "Error opening connection with " << usb_device  << std::endl;
			std::cout << "Check the connections with the CAO board" << std::endl;
			std::cout << "Trying again in 3 seconds" << std::endl;
			sleep(3);
		}
	}

	sonars_publisher = node_handle.advertise<cao_board::Sonars>("sonars", 10);

}

void CaoSensorNode::spin(){
	/*
	 * No more than 4 Hz, limited mainly by the timing of the sonars.
	 * Each one needs 70 ms to range, and some extra time for communication. With 3, this would be 210ms seconds only to range.
	 * Adding communication to it, and the other sensors, which also have their ADC convertion time, the total time could be 0,250 seconds.
	 */
	ros::Rate loopRate(4);
	while(ros::ok()){
		std::cout << "Writing: " << std::endl;
		// Back and front at the same time.
		communicator.writeTWIData(SONAR_C_L, WRITE_COMMAND, RANGE_CM);
		communicator.writeTWIData(SONAR_B_R, WRITE_COMMAND, RANGE_CM);
		usleep(35000);
		communicator.writeTWIData(SONAR_M_L, WRITE_COMMAND, RANGE_CM);
		communicator.writeTWIData(SONAR_S_R, WRITE_COMMAND, RANGE_CM);
		usleep(35000);
		communicator.writeTWIData(SONAR_M_R, WRITE_COMMAND, RANGE_CM);
		communicator.writeTWIData(SONAR_S_L, WRITE_COMMAND, RANGE_CM);
		usleep(35000);
		communicator.writeTWIData(SONAR_C_R, WRITE_COMMAND, RANGE_CM);
		communicator.writeTWIData(SONAR_B_L, WRITE_COMMAND, RANGE_CM);
		usleep(35000);

		std::cout << "Reading: " << std::endl;
		cao_board::Sonars message;

		message.sonar_c_l = measurementValue(SONAR_C_L);
		message.sonar_c_r = measurementValue(SONAR_C_R);
		message.sonar_m_l = measurementValue(SONAR_M_L);
		message.sonar_m_r = measurementValue(SONAR_M_R);
		message.sonar_b_l = measurementValue(SONAR_B_L);
		message.sonar_b_r = measurementValue(SONAR_B_R);
		message.sonar_s_l = measurementValue(SONAR_S_L);
		message.sonar_s_r = measurementValue(SONAR_S_R);

		sonars_publisher.publish(message);

		loopRate.sleep();
	}
}

int CaoSensorNode::measurementValue(unsigned char address){
	// Start variables.
	int low_byte = 0;
	int high_byte = 0;
	// Read them. If errors occur, these variables will receive negative values.
	high_byte = communicator.readTWIData(address, READ_HIGH_BYTE);
	low_byte = communicator.readTWIData(address, READ_LOW_BYTE);

	// If one is negative, don't trust is anymore, just return -1.
	if(high_byte < 0 || low_byte < 0)
		return -1;

	// Sum the values.
	int sum;
	sum = low_byte + (high_byte<<8);
	if(sum==0)
		return 255;
	else
		return sum;
}
